#include <iostream>
#include <termios.h>
#include <unordered_set>
#include <chrono>
#include <fcntl.h>
#include <iomanip>
#include <linux/fb.h>
#include <opencv2/opencv.hpp>
#include <sstream>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>
//#include "frame_buffer.h"

using namespace cv;
using namespace std;
using namespace std::chrono;

// 帧处理参数
const int targetWidth = 640;    //输入图像宽度
const int targetHeight = 480;   //输入图像高度
const int morphIterations = 2;  //形态学操作迭代次数

// 将 RGB888 转换为 RGB565
ushort rgb888_to_rgb565(const Vec3b& color) {
    return ((color[2] >> 3) << 11) | ((color[1] >> 2) << 5) | (color[0] >> 3);
}

// 获取当前时间字符串
string getCurrentTime() {
    auto now = system_clock::now();
    auto now_time_t = system_clock::to_time_t(now);
    auto now_tm = *localtime(&now_time_t);

    stringstream ss;
    ss << put_time(&now_tm, "%Y-%m-%d %H:%M:%S");
    return ss.str();
}

// 斑马线检测核心函数
void detectZebraCrossing(Mat& frame) {
    Mat gray, blurred, thresh, eroded, dilated;
    // 1. 灰度化处理
    cvtColor(frame, gray, COLOR_BGR2GRAY);
    // 2. 高斯滤波去噪 
    GaussianBlur(gray, blurred, Size(5, 5), 0);
    // 3. 自适应阈值处理
    adaptiveThreshold(blurred, thresh, 255, ADAPTIVE_THRESH_GAUSSIAN_C,THRESH_BINARY, 11, 2);
    // 4. 形态学操作（腐蚀+膨胀）
    Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 1));
    erode(thresh, eroded, kernel, Point(-1, -1), morphIterations);
    dilate(eroded, dilated, kernel, Point(-1, -1), morphIterations);   
    // 5. 轮廓检测与过滤
    vector<vector<Point>> contours;
    findContours(dilated, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

    for (const auto& cnt : contours) {
        Rect rect = boundingRect(cnt);
        float aspectRatio = (float)rect.width / rect.height;
        //过滤优化
        const int MIN_HEIGHT = 20;
        const float MIN_ASECT_RATIO = 0.5F;
        const int MIN_WIDTH = 30;
        const int MAX_WODTH = 150;
        // 基于斑马线特征的过滤条件
        if (rect.height > MIN_HEIGHT && aspectRatio > MIN_ASECT_RATIO && 
            rect.width > MIN_WIDTH && rect.width < MAX_WODTH) {

            // 绘制检测结果
            rectangle(frame, rect, Scalar(0, 255, 0), 2);
            //putText(frame, "Zebra", Point(rect.x, rect.y-5));
            putText(frame,
                    "Zebra",
                    Point(rect.x, rect.y-5),
                    FONT_HERSHEY_SIMPLEX,
                    0.5,
                    Scalar(0, 255, 0),
                    2);
        }
    }
}

int main() {
    VideoCapture cap(0);
    if (!cap.isOpened()) {
        cerr << "Error01" << endl;
        return -1;
    }

    cap.set(CAP_PROP_FRAME_WIDTH, 320);
    cap.set(CAP_PROP_FRAME_HEIGHT, 640);
    cap.set(CAP_PROP_FPS, 30);

    const char* fb_device = "/dev/fb0";
    int fb_fd = open(fb_device, O_RDWR);
    if (fb_fd == -1) {
        cerr << "Error02" << endl;
        return -1;
    }

    struct  fb_var_screeninfo vinfo;
    if (ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo)) {
        cerr << "Error03" << endl;
        close(fb_fd);
        return -1;
    }

    if (vinfo.bits_per_pixel != 16) {
        cerr << "Error04" << endl;
        close(fb_fd);
        return -1;
    }
    
    size_t fb_size = vinfo.xres * vinfo.yres * 2;
    ushort* fb_data = (ushort*)mmap(0, fb_size,
                                    PROT_READ | PROT_WRITE,
                                    MAP_SHARED, fb_fd, 0);
    if (fb_data == MAP_FAILED) {
        cerr << "Error04" << endl;
        close(fb_fd);
        return -1;
    }

    Mat rawFrame, displayFrame;
    int frameCounter = 0;
    auto startTime = steady_clock::now();
    while (true) {
        cap >> rawFrame;
        if (rawFrame.empty()) {
            cerr << "Error05" << endl;
            continue;
        }
    }

    Mat processFrame = rawFrame.clone();

    detectZebraCrossing(processFrame);

    //reprocessFrame, displayFrame,Size(vinfo.xres, vinfo.yres));
    resize(processFrame, displayFrame, Size(168 ,128));

    string timeStr = getCurrentTime();
    string frameStr = "Frame: " + to_string(frameCounter);

    // int fontFace = FONT_HERSHEY_SIMPLEX;
    // double fontScale = 0.4;
    // int thickness = 1;

    string infoText = " Loongson | " + getCurrentTime() + " FPS: " + to_string(frameCounter / duration_cast<seconds>(steady_clock::now() - startTime).count());

    // int baseLine = 0;
    // Size textSize = getTextSize(infoText, FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);

    //Point textOrg(10, textSize.height + 5);
    //putText(displayFrame, infoText, textOrg + Point(1,1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 0), 1);
    //putText(displayFrame, infoText, textOrg, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255,255,255),1);

    for (int y = 0; y < displayFrame.rows; ++y){
        for (int x = 0; x < displayFrame.cols; ++x) {
            Vec3b pixel = displayFrame.at<Vec3b>(y, x);
            fb_data[y * vinfo.xres + x] = rgb888_to_rgb565(pixel);
        }
    }

    frameCounter++;
    if (duration_cast<seconds>(steady_clock::now() - startTime).count() >= 1){
        startTime = steady_clock::now();
        frameCounter = 0;
    }


    munmap(fb_data, fb_size);
    close(fb_fd);
    cap.release();

    cout << "Close down" << endl;
    return 0;
}